syms Q1 Q2 Q3 Q4 Q5 Q6  d1 d4 d6  a2 a3

theta1=20;
theta2=20;
theta3=20;
theta4=20;
theta5=20;
theta6=20;
 
d1=90;
a2=355;
d4=396;
d6=70;

%将角度转化为弧度
Q1=theta1/180*pi;
Q2=theta2/180*pi;
Q3=theta3/180*pi;
Q4=theta4/180*pi;
Q5=theta5/180*pi;
Q6=theta6/180*pi;
T_01 =[ cos(Q1),    0,    sin(Q1),      0
        sin(Q1),    0,    -cos(Q1),     0
        0,          1,          0,      d1
        0,          0,          0,      1];

T_12 =[ cos(Q2),   -sin(Q2),    0,      a2*cos(Q2)
        sin(Q2),    cos(Q2),    0,      a2*sin(Q2)
        0,          0,          1,      0
        0,          0,          0,      1];

T_23 =[ cos(Q3),    0,    sin(Q3),      0
        sin(Q3),    0,    -cos(Q3),     0
        0,          1,          0,      0
        0,          0,          0,      1];

T_34 =[ cos(Q4),    0,    -sin(Q4),     0
        sin(Q4),    0,    cos(Q4),      0
        0,         -1,    0,           d4
        0,          0,    0,           1];

T_45 =[ cos(Q5),    0,    sin(Q5),    0
        sin(Q5),    0,    -cos(Q5),    0
        0,          1,    0,          0
        0,          0,    0,          1];

T_56 =[ cos(Q6),   -sin(Q6),    0,      0
        sin(Q6),   cos(Q6),     0   ,   0
        0,          0,          1,      d6
        0,          0,          0,      1];

T_06=T_01*T_12*T_23*T_34*T_45*T_56;
nx=T_06(1,1);
ny=T_06(2,1);
nz=T_06(3,1);
ox=T_06(1,2);
oy=T_06(2,2);
oz=T_06(3,2);
ax=T_06(1,3);
ay=T_06(2,3);
az=T_06(3,3);
px=T_06(1,4);
py=T_06(2,4);
pz=T_06(3,4);


              
disp('齐次变换矩阵正解：');
vpa(T_06,3)

disp('机器人工具箱正解：');
robot.fkine([Q1 Q2 Q3 Q4 Q5 Q6])
robot.plot([0,0,0,0,0,0]);